#include "shearcherMain.h"
#include <pthread.h>
#include <semaphore.h>

using namespace Eigen;

sem_t sem;

int _poly_num1D=8;
MatrixXd polyPath;

void pathFinding(Vector2d start_pt, Vector2d target_pt)
{
    AstarGraphSearch(start_pt, target_pt);
    std::vector<Vector2d> grid_path = getPath();
    MatrixXd motionPtah(grid_path.size(),2);
    for(int i=0;i<grid_path.size();i++)
    {
        motionPtah(i,0)=grid_path[i](0);
        motionPtah(i,1)=grid_path[i](1);
    }
    VectorXd _time = timeAllocation(motionPtah);
    MatrixXd vel = MatrixXd::Zero(2, 3); 
    MatrixXd acc = MatrixXd::Zero(2, 3);
    polyPath = PolyQPGeneration(4,motionPtah,vel,acc,_time);
    resetUsedGrids();
}

Vector2d getPosPoly( MatrixXd polyCoeff, int k, double t )
{
    Vector2d ret;

    for ( int dim = 0; dim < 2; dim++ )
    {
        VectorXd coeff = (polyCoeff.row(k)).segment( dim * _poly_num1D, _poly_num1D );
        VectorXd time  = VectorXd::Zero( _poly_num1D );
        
        for(int j = 0; j < _poly_num1D; j ++)
          if(j==0)
              time(j) = 1.0;
          else
              time(j) = pow(t, j);

        ret(dim) = coeff.dot(time);
        //cout << "dim:" << dim << " coeff:" << coeff << endl;
    }

    return ret;
}

Vector2d start_point,end_point;
bool searchComlite=false;

void* pathShearchThread(void*)
{
    sem_init(&sem,0,1);
    sem_wait(&sem);
    while(1)
    {
        sem_wait(&sem);
        searchComlite=false;
        pathFinding(start_point,end_point);
    }
    sem_destroy(&sem);
}

void startSearch(Vector2d start_pt, Vector2d target_pt)
{
    start_point=start_pt;
    target_pt=target_pt;
    sem_post(&sem);
}

MatrixXd getOptimizPath()
{
    MatrixXd blank(1,1);
    if(!searchComlite)
        return blank;
    searchComlite=false;
    return polyPath;
}
